import os
import sys
import time
from collections import Counter, deque
from typing import Deque, Optional, Tuple

import cv2

# Ensure local modules are importable when run from anywhere
REPO_ROOT = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
DETECT_DIR = os.path.join(REPO_ROOT, 'yolo-jetson', 'detect')
RS2_DIR = os.path.join(REPO_ROOT, 'yolo-jetson', 'rs2')
if DETECT_DIR not in sys.path:
    sys.path.append(DETECT_DIR)
if RS2_DIR not in sys.path:
    sys.path.append(RS2_DIR)

from yolo_pipe_ocr_hsv_final import create_ocr_hsv_pipeline  # noqa: E402
from GStreamerWrapper import GStreamerWrapper  # noqa: E402


def majority_vote(cache: Deque[Tuple[str, str, str]]) -> Optional[Tuple[str, str, str]]:
    if len(cache) == 0:
        return None
    merged = ['%s-%s-%s' % t for t in cache]
    top, _ = Counter(merged).most_common(1)[0]
    c, n, d = top.split('-')
    return c, n, d


def run(model_path: str,
        width: int = 1920,
        height: int = 1080,
        first_stage_conf: float = 0.6,
        final_conf: float = 0.6,
        top_cut_ratio: float = 0.25,
        width_center_ratio: float = 0.5,
        cache_size: int = 5,
        target_fps: int = 12) -> None:
    pipeline = create_ocr_hsv_pipeline(model_path=model_path, debug=False)

    gs = GStreamerWrapper(width=width, height=height)

    cache: Deque[Tuple[str, str, str]] = deque(maxlen=cache_size)
    frame_interval = 1.0 / max(1, target_fps)
    last_time = 0.0

    print('Starting GStreamer + YOLO OCR+HSV runner... Press Ctrl+C to stop.')

    try:
        while True:
            frame = gs.GetFrame()
            if frame is None:
                time.sleep(0.005)
                continue

            now = time.time()
            if now - last_time < frame_interval:
                time.sleep(0.001)
                continue
            last_time = now

            res = pipeline.process_image(
                frame,
                top_cut_ratio=top_cut_ratio,
                width_center_ratio=width_center_ratio,
                first_stage_conf=first_stage_conf,
                final_conf=final_conf,
            )

            cone = res.get('cone_color', 'None')
            num = res.get('number', 'None')
            dash = res.get('dashboard_state', 'None')

            if cone != 'None' and num != 'None' and dash != 'None':
                cache.append((cone, num, dash))
                print(f'Valid: {cone}-{num}-{dash} ({len(cache)}/{cache_size})')

            if len(cache) >= cache_size:
                final_triplet = majority_vote(cache)
                if final_triplet is not None:
                    fc, fn, fd = final_triplet
                    print(f'FINAL: {fc}-{fn}-{fd}')
                cache.clear()

    except KeyboardInterrupt:
        print('Interrupted by user.')
    finally:
        try:
            del gs
        except Exception:
            pass


if __name__ == '__main__':
    candidate_models = [
        os.path.join(DETECT_DIR, 'weights', 'all_yolo_all_epochs_0811', 'best.pt'),
        os.path.join(DETECT_DIR, 'weights', 'all_yolo_all_epochs_0808', 'best.pt'),
        os.path.join(DETECT_DIR, 'weights', 'all_yolo_all_epochs_0805', 'best.pt'),
        os.path.join(DETECT_DIR, 'weights', 'all_yolo_all_epochs_0804', 'best.pt'),
    ]
    model_path = None
    for p in candidate_models:
        if os.path.exists(p):
            model_path = p
            break
    if model_path is None:
        print('No model weights found. Please set model_path manually.')
        sys.exit(1)
    model_path = "/ultralytics/file/yolo_weight/yolo11n-200.engine"
    run(model_path=model_path)


